#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"
#include "hi_pwm.h"
#include "hi_timer.h"
#include "iot_pwm.h"
#include <time.h>

//左右两轮电机各由一个L9110S驱动
//GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车。
#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

//查阅机器人板原理图可知
//左边的红外传感器通过GPIO11与3861芯片连接
//右边的红外传感器通过GPIO12与3861芯片连接
#define GPIO11 11
#define GPIO12 12

#define GPIO_FUNC 0
#define car_speed_left 0
#define car_speed_right 0

#define DISTANCE_BETWEEN_CAR_AND_OBSTACLE 20.0  // 20cm障碍物检测距离忍受
extern float GetDistance(void);  // 获取超声波距离
extern unsigned char g_car_status;   
IotGpioValue io_status_left;
IotGpioValue io_status_right;

// 终点线相关模块
static time_t g_last_destination_time = 0;  // 上次检测到终点线的时间
static const int g_destination_timeout = 7;  // 终点线超时时间（单位：秒）
static int g_destination_counter = 0;  // 终点线检测计数
#define DESTINATION_COUNTER_MAX 3
// 新增：避障相关变量
static int g_obstacle_detected = 0;  // 障碍物检测标志
static int g_obstacle_check_counter = 0;  // 避障检测计数器

// 速度相关变量
extern double forward_speed;  // 前进速度
extern const double speed_max_change;  // 最大速度变化量
double distance = 0;
double last_distance = 0;

// 获取小车方向
typedef enum CarDirection {
    DIR_FORWARD = 1,
    DIR_BACKWARD = 2,
    DIR_LEFT = 3,
    DIR_RIGHT = 4,
    DIR_STOP = 5,
    DIR_BIG_LEFT = 6,
    DIR_BIG_RIGHT = 7,
}CarDir;
extern CarDir car_dir_index;
extern void car_run(CarDir);
CarDir car_dir_index_last;

//获取红外传感器的值，调整电机的状态
void timer1_callback(unsigned int arg)
{
    IotGpioValue io_status;
    // 每100ms检测一次障碍物
    g_obstacle_check_counter++;
    if (g_obstacle_check_counter >= 100) {
        g_obstacle_check_counter = 0;
        
        // 获取前方距离
        last_distance = distance;
        distance = GetDistance();
        if(car_dir_index == DIR_FORWARD){
            double now_speed = (last_distance-distance)*1;
            if(now_speed-forward_speed<speed_max_change && forward_speed-now_speed<speed_max_change){
                forward_speed=now_speed;
            }
        }
        else if (car_dir_index == DIR_STOP){
            forward_speed=0;
        }
        // 检查是否有障碍物
        if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {
            if (!g_obstacle_detected) {
                printf("🚨 Obstacle detected! Distance: %.2f cm\n", distance);
                g_obstacle_detected = 1;
                // 立即停止小车
                car_dir_index_last = car_dir_index;
                car_dir_index = DIR_STOP;
                forward_speed = 0;
            }
        } else {
            if (g_obstacle_detected) {
                printf("✅ Obstacle cleared! Distance: %.2f cm, resuming trace\n", distance);
                g_obstacle_detected = 0;
                car_dir_index = car_dir_index_last;
            }
        }
    }
    
    // 如果检测到障碍物，直接返回，不执行循迹逻辑
    if (g_obstacle_detected) {
        return;
    }

    IoTGpioGetInputVal(GPIO11,&io_status_left); //获取GPIO11引脚的输入电平值
    IoTGpioGetInputVal(GPIO12,&io_status_right);//获取GPIO12引脚的输入电平值
    
    //小车往左偏，则需要向右修正方向(右转)
    //如果GPIO12输入低电平（右边的红外传感器检测到黑色）并且GPIO11输入高电平（左边的红外传感器未检测到黑色）
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left != IOT_GPIO_VALUE0)
    {   
        car_dir_index = DIR_BIG_RIGHT;
    } 

    //小车往右偏，则需要向左修正方向（左转）
    //如果GPIO12输入高电平（右边的红外传感器未检测到黑色）并且GPIO11输入低电平（左边的红外传感器检测到黑色）
    if(io_status_right != IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        car_dir_index = DIR_BIG_LEFT;
    }
    //如果两个红外传感器都检测到黑色，则小车停止
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        //终点计数
        time_t current_time = time(NULL);
        if (current_time - g_last_destination_time >= g_destination_timeout) {
            g_destination_counter++;
            printf("Destination reached! Counter: %d\n", g_destination_counter);
            if(g_destination_counter >= DESTINATION_COUNTER_MAX){
                g_car_status = 0;
                forward_speed = 0;
            }
        }
        g_last_destination_time = current_time;
        forward_speed = 0;

    }
}


// void getCarDirection()
// {
//     if(g_car_speed_left == car_speed_left && g_car_speed_right == car_speed_right)
//     {
//         car_dir_index = DIR_FORWARD;
//     }
//     else if(g_car_speed_left == 1 && g_car_speed_right == car_speed_right)
//     {
//         car_dir_index = DIR_LEFT;
//     }
//     else if(g_car_speed_left == car_speed_left && g_car_speed_right == 1)
//     {
//         car_dir_index = DIR_RIGHT;
//     }
//     else if(g_car_speed_left == 1 && g_car_speed_right == 1)
//     {
//         car_dir_index = DIR_STOP;
//     }
//     // printf("car_dir_index = %d \r\n",car_dir_index);
// }

void trace_module(void)
{  
    regress_middle();
    g_destination_counter=0;
    unsigned int timer_id1;
    hi_timer_create(&timer_id1);
    unsigned int timer_id2;
    hi_timer_create(&timer_id2);
    unsigned int timer_id3;
    hi_timer_create(&timer_id3);

    //启动系统周期定时器用来按照预定的时间间隔1ms触发timer1_callback任务的执行
    hi_timer_start(timer_id1, HI_TIMER_TYPE_PERIOD, 1, timer1_callback, 0);
    // hi_timer_start(timer_id2, HI_TIMER_TYPE_PERIOD, 100, getCarDirection, 0);

    // gpio_control(GPIO0, IOT_GPIO_VALUE1);
    // gpio_control(GPIO1, g_car_speed_left);
    // gpio_control(GPIO9, IOT_GPIO_VALUE1);
    // gpio_control(GPIO10, g_car_speed_right);
    car_dir_index = DIR_FORWARD;
    
    while (1) { 
        // gpio_control(GPIO1, g_car_speed_left);
        // gpio_control(GPIO10, g_car_speed_right);
        car_run(car_dir_index);

        hi_udelay(20);

        if (g_car_status != CAR_TRACE_STATUS) {
            break;
        }
    }
    hi_timer_delete(timer_id1);
    hi_timer_delete(timer_id2);
    hi_timer_delete(timer_id3);
}